/***************************************************************************
 创建者: 华磊
 开始时间: 2019.8.2
 copyright: (C) 华友高科
 修改说明: (每次有修改就添加一条，带有 修改人，修改时间，修改描述)
 example (1) hualei 2020.5.8 类的具体实现代码编写
 ***************************************************************************/

#include "kalmanfiltersimple.h"

KalmanFilterSimple::KalmanFilterSimple(double followQ,double observeR)
{
    initial_kalmanFilter();
    setKalmanFilterPara(followQ,observeR);
}

int KalmanFilterSimple::initial_kalmanFilter()
{
    //2. 以高度为例 定义卡尔曼结构体并初始化参数
    kfp.LastP=0.02;//上次估算协方差 初始化值为0.02，变
    kfp.Now_P=0;//当前估算协方差 初始化值为0,变
    kfp.out=0;//卡尔曼滤波器输出 初始化值为0，变
    kfp.Kg=0;//卡尔曼增益 初始化值为0，变
    kfp.Q=0.001;//过程噪声协方差 初始化值为0.001。固定的
    kfp.R=0.0543;//观测噪声协方差 初始化值为0.543。固定的。越小收敛越快
    return 1;


}

double KalmanFilterSimple::calculate_kalmanFilter(double input)
{
    //预测协方差方程：k时刻系统估算协方差 = k-1时刻的系统协方差 + 过程噪声协方差
    kfp.Now_P = kfp.LastP + kfp.Q;
    //卡尔曼增益方程：卡尔曼增益 = k时刻系统估算协方差 / （k时刻系统估算协方差 + 观测噪声协方差）
    kfp.Kg=kfp.Now_P/(kfp.Now_P+kfp.R);
    //更新最优值方程：k时刻状态变量的最优值 = 状态变量的预测值 + 卡尔曼增益 * （测量值 - 状态变量的预测值）
    kfp.out = kfp.out + kfp.Kg * (input -kfp.out);//因为这一次的预测值就是上一次的输出值
    //更新协方差方程: 本次的系统协方差付给 kfp.LastP 威下一次运算准备。
    kfp.LastP = (1-kfp.Kg) * kfp.Now_P;
    return kfp.out;
}

int KalmanFilterSimple::setKalmanFilterPara(double followQ, double observeR)
{
    kfp.Q=followQ;//过程噪声协方差
    kfp.R=observeR;//观测噪声协方差
    return 1;
}
